#include "rclcpp/rclcpp.hpp"
// #include "control_base/WMJRobotControl.h"
#include "control/WMJRobotControl.h"
#include "std_msgs/msg/string.hpp"
#include "std_msgs/msg/float64.hpp"
#include <thread>
#include <tf2/LinearMath/Quaternion.h>
namespace wmj
{
    /*
     * @brief 设置速度与小陀螺状态
     *
     * @param: bool power 是否开启小陀螺（1为是，0为否）,  float x速度, float y速度，
     */
    void Control::setTopAndSpeedCallback(const base_interfaces::msg::Chassis::SharedPtr msg)
    {
        float_t x_speed = msg->x_speed;
        float_t y_speed = msg->y_speed;
        RobotControl->setTopAndSpeed(msg->power, msg->enable_cap, x_speed, y_speed);
    }

    /**
     * @brief 设置小陀螺状态
     *
     * @param base_interfaces::msg::Chassis::SharedPtr msg ,需设置msg->power （1为是，0为否）
     */
    void Control::switchTopStateCallback(const base_interfaces::msg::Chassis::SharedPtr msg)
    {
        bool use_top = msg->power;
        RobotControl->top(use_top);
    }

    void Control::setChassisSpeedCallback(const base_interfaces::msg::Chassis::SharedPtr msg)
    {
        float_t x_speed = msg->x_speed;
        float_t y_speed = msg->y_speed;
        RobotControl->setChassisSpeed(x_speed, y_speed);
    }

    /**
     * @brief 行为树小陀螺接口
     *
     * @param msg
     */
    // void Control::setTopCallback(const base_interfaces::msg::BtTop::SharedPtr msg)
    // {
    //     bool use_top = msg->start;
    //     RobotControl->top(use_top);
    //     RCLCPP_DEBUG(this->get_logger(), "top state: %s", [=]()
    //                  {if(use_top) return "open"; else return "close"; }());
    // }

    /**
     * @brief 发布当前速度
     *
     * 用base_interfaces::msg::Chassis msg 接收，话题名为函数名
     */
    void Control::publishChassisSpeed()
    {
        auto msg = base_interfaces::msg::Chassis();
        auto current_speed = RobotControl->getSpeed();
        msg.power = false;
        msg.x_speed = current_speed[0];
        msg.y_speed = current_speed[1];
        Control::m_getChassisSpeedPublisher->publish(msg);
    }

    /**
     * @brief 发布当前小陀螺状态
     *
     * 用 base_interfaces::msg::Chassis msg接收，话题名为函数名
     */
    void Control::publishTopState()
    {
        auto msg = base_interfaces::msg::Chassis();
        auto current_top = RobotControl->get_top();
        msg.power = current_top;
        msg.x_speed = 0.0f;
        msg.y_speed = 0.0f;
        Control::m_getTopStatePublisher->publish(msg);
    }

    void Control::navigationSubscriptionCallback(const base_interfaces::msg::BtNavigation::SharedPtr msg)
    {
        // if (msg->navigation_continue)
        //     m_gimbalControlPermissions = GIMBAL_CONTROL_PERMISSION::NAVIGATION;
    }

    template<typename T1, typename T2>
    void addPublisher(std::string topic_name, int simple_qos, T2 callback, Control* p)
    {
        auto pub = p->create_publisher<T1>(topic_name, simple_qos);
        p->m_pub_vec.push_back(pub);
        std::function<void()> timer_callback = callback;
        auto pub_timer = p->create_wall_timer(5ms, timer_callback);
        p->m_pub_timer_vec.push_back(pub_timer);
    }

    // std::function<void(std::shared_ptr<T>)>
    template<typename T1, typename T2>
    void addSubscription(std::string topic_name, int simple_qos, T2 callback, Control* p)
    {
        std::function<void(std::shared_ptr<T1>)> sub_callback = callback;
        auto sub = p->create_subscription<T1>(topic_name, simple_qos, sub_callback);
        p->m_sub_vec.push_back(sub);
    }

    /***********************************构造函数***************************************/
    Control::Control(std::string name, std::shared_ptr<wmj::WMJRobotControl> _RobotControl) : Node(name)
    {
        if (!_RobotControl)
        {
            RobotControl = std::make_shared<wmj::WMJRobotControl>();
            usleep(1e6);
        }
        // 云台
        // 实例化，操控云台的，话题名称都是函数名

        // m_setGimbalAngleSubscription = this->create_subscription<base_interfaces::msg::GimbalPose>("SetGimbalAngle", 10, std::bind<void(wmj::Control::*)(const base_interfaces::msg::GimbalPose::SharedPtr, GIMBAL_ORIENTATION)>(&Control::setGimbalAngleCallback, this, _1, LEFT));
        // m_setGimbalAngleSubscription = this->create_subscription<base_interfaces::msg::GimbalPose>("SetGimbalAngle", 10, std::bind(&Control::setGimbalAngleCallback, this, _1));
        // m_setGimbalAngleSubscriptionL = this->create_subscription<base_interfaces::msg::GimbalPose>("/left/SetGimbalAngle", 10, std::bind(&Control::setGimbalAngleCallbackL, this, _1));
        // m_setGimbalAngleSubscriptionR = this->create_subscription<base_interfaces::msg::GimbalPose>("/right/SetGimbalAngle", 10, std::bind(&Control::setGimbalAngleCallbackR, this, _1));


        // m_setGimbalSpeedSubscription = this->create_subscription<base_interfaces::msg::GimbalPose>("SetGimbalSpeed", 10, std::bind(&Control::setGimbalSpeedCallback, this, _1, _2));

        // m_setGimbal_YawSpeed_PitchAngle_Subscription = this->create_subscription<base_interfaces::msg::GimbalPose>("SetGimbal_YawSpeed_PitchAngle", 10, std::bind(&Control::setGimbal_YawSpeed_PitchAngle_Callback, this, _1));
        // 原获取GimbalPose函数，一个发布者

        m_getGimbalAnglePublisher = this->create_publisher<base_interfaces::msg::GimbalPose>("GetGimbalAngle", 10);
        m_getGimbalSpeedPublisher = this->create_publisher<base_interfaces::msg::GimbalPose>("GetGimbalSpeed", 10);

        // 时间回调函数，定时发布
        // m_getGimbalAnglePublisherTimer = this->create_wall_timer(5ms, std::bind(&Control::publishGimbalAngle, this));
        // m_getGimbalSpeedPublisherTimer = this->create_wall_timer(10ms, std::bind(&Control::publishGimbalSpeed, this));

        // 硬触发的，服务形式,获得数据并返回
        m_getGimbalAngleTrigrtService = this->create_service<base_interfaces::srv::GimbalPose>("GetGimbalAngle_Trigger", std::bind(&Control::getGimbalAngleTriggerCallback, this, _1, _2), rmw_qos_profile_default);
        // 开启硬触发
        m_switchTriggerStateService = this->create_service<base_interfaces::srv::Trigger>("Trigger", std::bind(&Control::switchTriggerStateCallback, this, _1, _2), rmw_qos_profile_default);
        // 底盘
        // 接受者，控底盘的
        // m_setChassisSpeedSubscription = this->create_subscription<base_interfaces::msg::Chassis>("setChassisSpeed", 10, std::bind(&Control::setChassisSpeedCallback, this, _1));
        m_setTopAndSpeedSubscription = this->create_subscription<base_interfaces::msg::Chassis>("TopAndSpeed", 10, std::bind(&Control::setTopAndSpeedCallback, this, _1));
        // m_switchTopStateSubscription = this->create_subscription<base_interfaces::msg::Chassis>("top", 10, std::bind(&Control::switchTopStateCallback, this, _1));
        // m_setTopSubscription = this->create_subscription<base_interfaces::msg::BtTop>("BT_Top", 10, std::bind(&Control::setTopCallback, this, _1));
        // 发布者，发布底盘信息的
        m_getChassisSpeedPublisher = this->create_publisher<base_interfaces::msg::Chassis>("getSpeed", 10);
        m_getTopStatePublisher = this->create_publisher<base_interfaces::msg::Chassis>("get_top", 10);
        m_getChassisSpeedPublisherTimer = this->create_wall_timer(100ms, std::bind(&Control::publishChassisSpeed, this));
        m_getTopStatePublisherTimer = this->create_wall_timer(100ms, std::bind(&Control::publishTopState, this));
        // 射击
        // 接收者
        // m_shootSomeSubscription = this->create_subscription<base_interfaces::msg::Shooter>("ShootSome", 10, std::bind(&Control::shootSomeCallback, this, _1));
        m_stopShootSubscription = this->create_subscription<base_interfaces::msg::Shooter>("StopShoot", 10, std::bind(&Control::stopShootCallback, this, _1));
        m_openBoxSubscription = this->create_subscription<base_interfaces::msg::Shooter>("openBox", 10, std::bind(&Control::openBoxCallback, this, _1));
        //!
        m_shootControlSubscription = this->create_subscription<base_interfaces::msg::BtAimer>("BT_shooter", 10, std::bind(&Control::shootControlCallback, this, _1));
        // 发布者
        m_highShootSpeedPublisher = this->create_publisher<base_interfaces::msg::Shooter>("HighShootSpeed", 10);
        m_getBulletNumberPublisher = this->create_publisher<base_interfaces::msg::Shooter>("GetBulletNumber", 10);
        m_highShootSpeedPublisherTimer = this->create_wall_timer(100ms, std::bind(&Control::highShootSpeedCallback, this));
        m_getBulletNumberPublisherTimer = this->create_wall_timer(100ms, std::bind(&Control::getBulletNumberCallback, this));
        // 车间
        // 发布者
        m_recvRobotMsgPublisher = this->create_publisher<base_interfaces::msg::Communicate>("recvFromRobot", 10);
        m_recvRobotMsgPublisherTimer = this->create_wall_timer(100ms, std::bind(&Control::publishRobotMessage, this));
        // 接收者
        m_sendRobotMsgSubscription = this->create_subscription<base_interfaces::msg::Communicate>("sendToRobot", 10, std::bind(&Control::sendRobotMsgCallback, this, _1));

        // Gamestate
        m_gameStatePublisher = this->create_publisher<base_interfaces::msg::Game>("Game", 10);
        m_gameStatePublishTimer = this->create_wall_timer(100ms, std::bind(&Control::publishRobotStateMassage, this));
        // Navigation
        m_navigationSubscription = this->create_subscription<base_interfaces::msg::BtNavigation>("BT_navigation", 10, std::bind(&Control::navigationSubscriptionCallback, this, _1));
        m_navigationGoalPublisher = this->create_publisher<geometry_msgs::msg::PoseStamped>("/goal_pose", 1);
        m_navigationGoalPublisherTimer = this->create_wall_timer(100ms, std::bind(&Control::publishNavigationGoalCallback, this));
        m_keySubscription = this->create_subscription<std_msgs::msg::Char>("my_key", 10, std::bind(&Control::keySubscription, this, _1));
        // Scan
        m_scanSubscription = this->create_subscription<base_interfaces::msg::ScanCtrlInfo>("ScanCtrlInfo", 10, std::bind(&Control::scanSubscriptionCallback, this, _1));
        RobotControl->SwitchBaseCoor(1);
	    //RobotControl->top(1);
        m_jointStatePublisher = this->create_publisher<sensor_msgs::msg::JointState>("/joint_states", 1);
    
        /////////////////////////////////////////////////////////////////////
        addSubscription<base_interfaces::msg::GimbalPose>("/left/setGimbalAngle", 10, std::bind(&Control::setGimbalAngleCallback, this, _1, LEFT), this);
        addSubscription<base_interfaces::msg::GimbalPose>("/right/setGimbalAngle", 10, std::bind(&Control::setGimbalAngleCallback, this, _1, RIGHT), this);
        addSubscription<base_interfaces::msg::GimbalPose>("/left/setGimbalSpeed", 10, std::bind(&Control::setGimbalSpeedCallback, this, _1, LEFT), this);
        addSubscription<base_interfaces::msg::GimbalPose>("/right/setGimbalSpeed", 10, std::bind(&Control::setGimbalSpeedCallback, this, _1, RIGHT), this);
        addSubscription<base_interfaces::msg::GimbalPose>("/left/setGimbal_pitchAngle_yawSpeed", 10, std::bind(&Control::setGimbal_YawSpeed_PitchAngle_Callback, this, _1, LEFT), this);
        addSubscription<base_interfaces::msg::GimbalPose>("/right/setGimbal_pitchAngle_yawSpeed", 10, std::bind(&Control::setGimbal_YawSpeed_PitchAngle_Callback, this, _1, RIGHT), this);
        addSubscription<base_interfaces::msg::BigYaw>("/setBigYaw", 10, std::bind(&Control::setBigYawCallback, this, _1), this);
        addSubscription<base_interfaces::msg::Shooter>("/left/shootSome", 10, std::bind(&Control::shootSomeCallback, this, _1, LEFT), this);
        addSubscription<base_interfaces::msg::Shooter>("/right/shootSome", 10, std::bind(&Control::shootSomeCallback, this, _1, RIGHT), this);
        addSubscription<base_interfaces::msg::BtJudge>("/BtJudge", 10, std::bind(&Control::BehaviorTreeCallBack, this, _1), this);

        addPublisher<base_interfaces::msg::GimbalPose>("/right/getGimbalAngle", 5, std::bind(&Control::publishGimbalAngle2, this, 0, RIGHT), this);
        addPublisher<base_interfaces::msg::GimbalPose>("/left/getGimbalAngle", 5, std::bind(&Control::publishGimbalAngle2, this, 1, LEFT), this);
        // addPublisher<base_interfaces::msg::GimbalPose>("/left/getGimbalPoseAngle", 10, std::bind(&Control::publishGimbalPoseAngleCallback, this, 2, LEFT), this);
        // addPublisher<base_interfaces::msg::GimbalPose>("/right/getGimbalPoseAngle", 10, std::bind(&Control::publishGimbalPoseAngleCallback, this, 3, RIGHT), this);
        // addPublisher<base_interfaces::msg::GimbalPose>("/left/getGimbalPoseSpeed", 10, std::bind(&Control::publishGimbalPoseSpeedCallback, this, 4, LEFT), this);
        // addPublisher<base_interfaces::msg::GimbalPose>("/right/getGimbalPoseSpeed", 10, std::bind(&Control::publishGimbalPoseSpeedCallback, this, 5, RIGHT), this);
        addPublisher<base_interfaces::msg::ScanCtrlInfo>("/left/scanRange", 10, std::bind(&Control::publishGimbalRotationRangeCallback, this, 2, LEFT), this);
        addPublisher<base_interfaces::msg::ScanCtrlInfo>("/right/scanRange", 10, std::bind(&Control::publishGimbalRotationRangeCallback, this, 3, RIGHT), this);
        addPublisher<base_interfaces::msg::BigYaw>("/getBigYaw", 10, std::bind(&Control::publishBigYawCallback, this, 4), this);
        addPublisher<std_msgs::msg::Float32>("/left/getBulletSpeed", 5, std::bind(&Control::publishBulletSpeed, this, 5, LEFT), this);

    }
}
